#include "limit_process.h"
#include "control.h"
#include "process_inputs.h"
extern STRUCT_CONTROL Ct;

static int lmProcessSoftPositLimit(double posit[MAX_JOINT], float vel[MAX_JOINT], float acc[MAX_JOINT]);
static int lmProcessAbsPositLimit(double posit[MAX_JOINT], float vel[MAX_JOINT], float acc[MAX_JOINT]);

int lmProcessSoftPositLimit(double posit[MAX_JOINT], float vel[MAX_JOINT], float acc[MAX_JOINT])
{
	int i = 0;
	for (i = 0; i < MAX_JOINT; i++){
		if (Ct.conf.joint_enable[i]){
			if (posit[i] > Ct.conf.limit_soft.limit_h[i] && vel[i] > 0){
				Ct.soft_limit_flag = 1;
				Controller_MsgVp(MOTION_WARN_POSIT_OVER_SOFT_LIMIT, " ");
				DWarn("soft posit limit,joint=%d,posit=%f,limit=%f,vel=%f",
					i, posit[i], Ct.conf.limit_soft.limit_h[i], vel[i]);
				Cmd_ParseCommand_CMD_ABORT();
				return 0;
			}
			else if (posit[i] < Ct.conf.limit_soft.limit_l[i] && vel[i] < 0){
				Ct.soft_limit_flag = 1;
				Controller_MsgVp(MOTION_WARN_POSIT_OVER_SOFT_LIMIT, " ");
				DWarn("soft posit limit,joint=%d,posit=%f,limit=%f,vel=%f",
					i,posit[i], Ct.conf.limit_soft.limit_l[i], vel[i]);
				Cmd_ParseCommand_CMD_ABORT();
				return 0;
			}
		}
	}
	return 0;
}

int lmProcessAbsPositLimit(double posit[MAX_JOINT], float vel[MAX_JOINT], float acc[MAX_JOINT])
{
	int i = 0;
	for (i = 0; i < MAX_JOINT; i++){
		if (Ct.conf.joint_enable[i]){
			if (posit[i] > Ct.conf.limit_abs.limit_h[i] && vel[i] > 0){
				Ct.abs_limit_flag = 1;
				Controller_MsgVp(MOTION_ERROR_POSIT_OVER_ABS, " ");
				DWarn("abs posit limit,joint=%d,posit=%f,limit=%f,vel=%f",
					i, posit[i], Ct.conf.limit_abs.limit_h[i], vel[i]);
				return 0;
			}
			else if (posit[i] < Ct.conf.limit_abs.limit_l[i] && vel[i] < 0){
				Ct.abs_limit_flag = 1;
				Controller_MsgVp(MOTION_ERROR_POSIT_OVER_ABS, " ");
				DWarn("abs posit limit,joint=%d,posit=%f,limit=%f,vel=%f",
					i, posit[i], Ct.conf.limit_abs.limit_l[i], vel[i]);
				return 0;
			}
		}
	}
	return 0;
}

int lmProcess(double posit[MAX_JOINT], float vel[MAX_JOINT], float acc[MAX_JOINT])
{
	if (!Ct.soft_limit_flag){
		lmProcessSoftPositLimit(posit, vel, acc);
	}
	if (!Ct.abs_limit_flag){
		lmProcessAbsPositLimit(posit, vel, acc);
	}
	return 0;
}

